/*
 * BGFGSegmentor.cpp
 *
 *  Created on: Nov 24, 2013
 *      Author: thanhkm
 */
#include "TrackingPeople.h"

int main(int argc, char** argv) {
	cv::VideoCapture cap;
	// read video
	if (argc > 1)
		cap.open(argv[1]);
	else
		cap.open(0);
	// check if it is open
	if (!cap.isOpened()) {
		std::cerr
				<< "Cannot read video. Try moving video file to sample directory."
				<< std::endl;
		return -1;
	}

	//trace of people in the scence
	cv::Mat trace;
	// current video frame
	cv::Mat frame;
	cv::Mat gray;
	// foreground binary image
	cv::Mat foreground;
	cv::Mat background;
	cv::Mat boxImage;
	int idx = 10;
	// Vector of rectangle around objet
	vector<Rect> boundRect;

	// List of peole detected in the scence
	vector<People> vectPeople;

	//---initialize trace
	if (!cap.read(frame))
		return -1;
	trace = Mat::zeros(frame.size(), CV_8UC3);

	// The Mixture of Gaussian object
	// used with all default parameters
	cv::BackgroundSubtractorMOG mog;
	bool stop(false);
	// for all frames in video
	while (!stop) {
		// read next frame if any
		if (!cap.read(frame))
			break;
		//
		cvtColor(frame, gray, CV_BGR2GRAY);

		GaussianBlur(gray, gray, Size(3, 3), 1.7, 1.7);
//              cv::imshow("gray", gray);
		// update the background
		// and return the foreground
		mog(gray, foreground, 0.01);


		dilate(foreground, foreground, Mat(), Point(-1, -1), 1);
		erode(foreground, foreground, Mat(), Point(-1, -1), 3);
		dilate(foreground, foreground, Mat(), Point(-1, -1), 2);

		// Complement the image
		cv::threshold(foreground, foreground, 128, 255, cv::THRESH_BINARY);
		// show foreground
		cv::imshow("Extracted Foreground", foreground);

		// calculate the bound of foreground with merge
		// small regions
		boundRect = mergeContours(foreground, frame);
		//clone image
		boxImage = frame.clone();

		for (uint i = 0; i < boundRect.size(); i++) {
//                      Scalar color = Scalar(30, 60, 200);
//                      rectangle(boxImage, boundRect[i], color, 2, 8, 0);
			cout << "number of people: " << vectPeople.size() << endl;

			//check for create or update people
			bool isNewPeople = true;
			if (vectPeople.size() < 1) {
				People p(i, frame, boundRect[i].tl(), boundRect[i].br());
				vectPeople.push_back(p);
			} else if (vectPeople.size() >= 1) {
				//create new
				// check for list people detect
				// if no one match create new people
				// if matching, update state
				for (uint j = 0; j < vectPeople.size(); j++) {
					if (vectPeople[j].isMatchedPeople(frame, boundRect[i].tl(),
							boundRect[i].br())) {
//                                              cout<<"match"<<endl;
						//update
						vectPeople[j].update(frame, boundRect[i].tl(),
								boundRect[i].br());
						cout << "is in scence:"
								<< vectPeople[j].isInScence(frame) << endl;
						vectPeople[j].setUpdated(true);

						isNewPeople = false;
						break;
					}
				}
				if (isNewPeople) {
					cout << "create new people" << endl;
					People p(i, frame, boundRect[i].tl(), boundRect[i].br());
					vectPeople.push_back(p);

				}
			}
		}

		//Delete object not in scene
		vector<People>::iterator it = vectPeople.begin();
		while (it != vectPeople.end()) {
			if (!(*it).isInScence(frame)) {
				it = vectPeople.erase(it);
				cout << "Delete people=================" << endl;
			} else {
				++it;
			}
		}

		//Draw object
		for (uint i = 0; i < vectPeople.size(); i++) {
			Scalar color = vectPeople[i].getColor();
			Rect rect = vectPeople[i].getRect();

			rectangle(boxImage, rect, color, 2, 8, 0);

			//
			vector<Point2i> vcorr = vectPeople[i].getvCor();
			vector<Point2i> vpre = vectPeople[i].getvPre();
			vector<Point2i> vmer = vectPeople[i].getvMer();

			if (vcorr.size()>2){
				Point pt1 = vcorr[vcorr.size()-1];
				Point pt2 = vcorr[vcorr.size()-2];
				line(trace, pt2, pt1, Scalar(0,0,255), 2);
			}

			if (vpre.size() > 2) {
				Point pt1 = vpre[vpre.size() - 1];
				Point pt2 = vpre[vpre.size() - 2];
				line(trace, pt2, pt1, Scalar(255, 0, 0), 2);
			}

			if (vmer.size() > 2) {
				Point pt1 = vmer[vmer.size() - 1];
				Point pt2 = vmer[vmer.size() - 2];
				line(trace, pt2, pt1, Scalar(0, 255, 0), 2);
			}

		}
		imshow("Result", boxImage);
		imshow("Trace", trace);
		// introduce a delay
		// or press key to stop

		if (cv::waitKey(10) == 32){
			string s = "fg";
			string s1 = "video";
			string s2 = "trace";
			ostringstream convert;   // stream used for the conversion
			convert << idx;      // insert the textual representation of 'Number' in the characters in the stream
			string result = convert.str(); // set 'Result' to the contents of the stream

			s.append(result);
			s1.append(result);
			s2.append(result);
			s.append(".jpg");
			s1.append(".jpg");
			s2.append(".jpg");
			imwrite(s, foreground);
			imwrite(s1, boxImage);
			imwrite(s2, trace);
			idx++;
		} else if (cv::waitKey(10) == 27){
			stop = true;
		}
	}

//	waitKey(0);
	return 0;
} //--End main

// Check list people
vector<People> checkList(Mat frame, vector<Rect> boundRect) {
	vector<People> vectPeople;

	return vectPeople;
}

// Merge contours
// funsionner les contours qui sont proches et assez petits
// idee:
// 1,get all contours,
// 2,find near contours by distance of point or center,
// 3,Merge
// return
vector<Rect> mergeContours(Mat imgmsk, Mat &img2) {

	Mat threshold_output;
	vector<vector<Point> > contours;
	vector<vector<Point> > m_contours;
	vector<Vec4i> hierarchy;

	/// Detect edges using Threshold
	threshold(imgmsk, threshold_output, 20, 255, THRESH_BINARY);
	/// Find contours
	findContours(threshold_output, contours, hierarchy, CV_RETR_TREE,
			CV_CHAIN_APPROX_SIMPLE, Point(0, 0));

	// Approximate contours to polygons + get bounding rects and circles
	vector<vector<Point> > contours_poly(contours.size());
	vector<Rect> boundRect(contours.size());
	// bound rectangle of merged contours
	vector<Rect> m_boundRect;

	// make rectangle around the contours
	for (uint i = 0; i < contours.size(); i++) {
		approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);
		boundRect[i] = boundingRect(Mat(contours_poly[i]));
	}

	float seuil = 250;
	// Get the big contours which has area greater than seuil (15)
	for (uint i = 0; i < contours.size(); i++) {
		if (boundRect[i].area() > seuil) {
			// saved big contours and the rectangle around it
			m_contours.push_back(contours[i]);
			m_boundRect.push_back(boundRect[i]);
		}
	}
	// merged small contours with the nearest big contours
	for (uint i = 0; i < contours.size(); i++) {
		if (boundRect[i].area() < seuil) {

			Point2i center = getMidPoint(boundRect[i].tl(), boundRect[i].br());

			float distance = 19.0, d_temp = 0.0;
			int idx = -1;

			// search the nearest contours
			for (uint j = 0; j < m_contours.size(); j++) {
				Point2i m_center = getMidPoint(m_boundRect[i].tl(),
						m_boundRect[i].br());
				d_temp = dist(center, m_center);
				if (d_temp < distance) {
					distance = d_temp;
					idx = j;
				}
			}

			// push the small contours in the nearest
			if (idx != -1 && distance < 7.0) {
				for (uint k = 0; k < contours[i].size(); k++) {
					m_contours[idx].push_back(contours[i][k]);
				}
			}
		}
	}

	// Approximate contours to polygons + get bounding rects and circles
	vector<vector<Point> > m_contours_poly(m_contours.size());
	vector<Rect> merged_boundRect(m_contours.size());

	// make rectangle around the contours
	for (uint i = 0; i < m_contours.size(); i++) {
		approxPolyDP(Mat(m_contours[i]), m_contours_poly[i], 3, true);
		merged_boundRect[i] = boundingRect(Mat(m_contours_poly[i]));
	}

	return merged_boundRect;
}

// calcul distance of two point
float dist(Point2f a, Point2f b) {
	return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}

// Get midpoint of two points
Point2i getMidPoint(Point2i a, Point2i b) {
	return Point2i((a.x + b.x) / 2, (a.y + b.y) / 2);
}

//get a region of image
Mat getImageRegion(Mat img, Point2i tl, Point2i br) {
	return img(Rect(tl, br));
}

// make box of image
vector<Rect> makeBoxImage(Mat imgmsk, Mat img2) {

	Mat threshold_output;
	vector<vector<Point> > contours;
	vector<Vec4i> hierarchy;

	/// Detect edges using Threshold
	threshold(imgmsk, threshold_output, 20, 255, THRESH_BINARY);
	/// Find contours
	// CV_RETR_ETERNAL and the method CV_LINK_RUNS.
	findContours(threshold_output, contours, hierarchy, CV_RETR_TREE,
			CV_CHAIN_APPROX_SIMPLE, Point(0, 0));

	/// Approximate contours to polygons + get bounding rects and circles
	vector<vector<Point> > contours_poly(contours.size());
	vector<Rect> boundRect(contours.size());

	for (uint i = 0; i < contours.size(); i++) {
		approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);
		boundRect[i] = boundingRect(Mat(contours_poly[i]));
	}

	RNG rng(1222443);
	/// Draw polygonal contour + bonding rects
	Mat drawing = img2.clone();
	bool isWrite = false;

	for (uint i = 0; i < contours.size(); i++) {
		Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255),
				rng.uniform(0, 255));
		if (dist(boundRect[i].tl(), boundRect[i].br()) > 19) {
			rectangle(drawing, boundRect[i].tl(), boundRect[i].br(), color, 2,
					8, 0);
//                      drawContours(drawing, contours, i,color, 2, 8, hierarchy, 0, Point());
			if (dist(boundRect[i].tl(), boundRect[i].br()) > 25 && !isWrite) {
				imwrite("ob.jpg",
						getImageRegion(img2, boundRect[i].tl(),
								boundRect[i].br()));
				isWrite = true;
			}
		}
	}

	/// Show in a window
	namedWindow("boxContours", CV_WINDOW_AUTOSIZE);
	imshow("boxContours", drawing);
	return boundRect;
}

